package legoproject.Behaviors;

import legoproject.*;
import java.util.ArrayList;

public class PickUpGarbage implements Behavior {

    private int distanceToTGarbage = 11;
    private int[] list = new int[] {30, 30};
    private int currentElement = 0;


    public boolean condition() {

        if(GarbageCollector.garbageInGrab){
            return false;
        }

        int input = GarbageCollector.ultrasonicSensor.getDistance();
        GarbageCollector.reporter.updateStat(0, input);
        list[currentElement] = input;

        currentElement = (currentElement + 1) % 2;
        int touch = 0;
        try{touch = GarbageCollector.bluetoothManager.sendCommand(CommandType.READTOUCH);}catch(Exception e){}


        return ((touch == 1 || checkList()) && !GarbageCollector.garbageInGrab);

    }

    public ArrayList behave() {
        ArrayList result = new ArrayList();

        result.add(new Instruction(InstructionType.DRIVE, 200, 50));
        result.add(new Instruction(InstructionType.GRAB, 0, 0));
        return result;
    }

    private boolean checkList(){
        int temp = (distanceToTGarbage);
        return list[0] < temp && list[1] < temp;
    }
}